//  @ Project : SSU Robot Simulator
//  @ File Name : Robot.cpp
//  @ Date : 4/4/2011
//  @ Author : Andrew Holbrook
//
//

#include "SSURoboSim/Kinematics/Robot.h"
#include "SSURoboSim/Kinematics/Joint.h"
#include "SSURoboSim/Kinematics/PrismaticJoint.h"
#include "SSURoboSim/Kinematics/RevoluteJoint.h"

using SSURoboSim::Kinematics::Robot;
using SSURoboSim::Kinematics::Joint;
using SSURoboSim::Kinematics::PrismaticJoint;
using SSURoboSim::Kinematics::RevoluteJoint;

Robot::Robot()
{
}

Robot::~Robot()
{
	vector<Joint *>::iterator it;

	for (it = joints_.begin(); it != joints_.end(); ++it)
	{
		delete *it;
	}
}

void Robot::addPrismaticJoint(Joint &j)
{
	PrismaticJoint *prismJoint = new PrismaticJoint(j);
	joints_.push_back(prismJoint);
}

void Robot::addRevoluteJoint(Joint &j)
{
	RevoluteJoint *revJoint = new RevoluteJoint(j);
	joints_.push_back(revJoint);
}

void Robot::glTransform(unsigned int f)
{
	unsigned int i;

	for (i = 0; i < f; ++i)
	{
		frames_[i].glTransform();
		joints_[i]->glJointTransform();
	}
}

void Robot::update(unsigned int dtime)
{
	float val = joints_[1]->getValue();
	val += (360.0f / 1000.0f) * dtime;
	if (val > 360.0f)
	{
		val -= 360.0f;
	}

	joints_[1]->setValue(val);

	val = joints_[2]->getValue();
	val += (360.0f / 2000.0f) * dtime;
	if (val > 360.0f)
	{
		val -= 360.0f;
	}

	joints_[2]->setValue(val);
}
